#include "plan_env/grid_map.h"

void GridMap::initMap(ros::NodeHandle &nh)
{
  node_ = nh;

  /* get parameter */
  double x_size, y_size, z_size;
  node_.param("grid_map/resolution", gmp_.resolution_, -1.0);
  node_.param("grid_map/map_size_x", x_size, -1.0);
  node_.param("grid_map/map_size_y", y_size, -1.0);
  node_.param("grid_map/map_size_z", z_size, -1.0);
  node_.param("grid_map/local_update_range_x", gmp_.local_update_range_(0), -1.0);
  node_.param("grid_map/local_update_range_y", gmp_.local_update_range_(1), -1.0);
  node_.param("grid_map/local_update_range_z", gmp_.local_update_range_(2), -1.0);
  node_.param("grid_map/obstacles_inflation", gmp_.obstacles_inflation_, -1.0);

  node_.param("grid_map/fx", gmp_.fx_, -1.0);
  node_.param("grid_map/fy", gmp_.fy_, -1.0);
  node_.param("grid_map/cx", gmp_.cx_, -1.0);
  node_.param("grid_map/cy", gmp_.cy_, -1.0);

  node_.param("grid_map/use_depth_filter", gmp_.use_depth_filter_, true);
  node_.param("grid_map/depth_filter_tolerance", gmp_.depth_filter_tolerance_, -1.0);
  node_.param("grid_map/depth_filter_maxdist", gmp_.depth_filter_maxdist_, -1.0);
  node_.param("grid_map/depth_filter_mindist", gmp_.depth_filter_mindist_, -1.0);
  node_.param("grid_map/depth_filter_margin", gmp_.depth_filter_margin_, -1);
  node_.param("grid_map/k_depth_scaling_factor", gmp_.k_depth_scaling_factor_, -1.0);
  node_.param("grid_map/skip_pixel", gmp_.skip_pixel_, -1);

  node_.param("grid_map/p_hit", gmp_.p_hit_, 0.70);
  node_.param("grid_map/p_miss", gmp_.p_miss_, 0.35);
  node_.param("grid_map/p_min", gmp_.p_min_, 0.12);
  node_.param("grid_map/p_max", gmp_.p_max_, 0.97);
  node_.param("grid_map/p_occ", gmp_.p_occ_, 0.80);
  node_.param("grid_map/min_ray_length", gmp_.min_ray_length_, -0.1);
  node_.param("grid_map/max_ray_length", gmp_.max_ray_length_, -0.1);

  node_.param("grid_map/visual_truncate_height", gmp_.visual_truncate_height_, -0.1);
  node_.param("grid_map/virtual_ceil_height", gmp_.virtual_ceil_height_, -0.1);

  node_.param("grid_map/pose_type", gmp_.pose_type_, 1);

  node_.param("grid_map/frame_id", gmp_.frame_id_, string("world"));
  node_.param("grid_map/local_map_margin", gmp_.local_map_margin_, 1);
  node_.param("grid_map/ground_height", gmp_.ground_height_, 1.0);

  node_.param("grid_map/odom_depth_timeout", gmp_.odom_depth_timeout_, 1.0);

  node_.param("grid_map/esdf_x_bound", gmp_.esdf_x_bound_, 1);
  node_.param("grid_map/esdf_y_bound", gmp_.esdf_y_bound_, 1);
  node_.param("grid_map/esdf_z_bound", gmp_.esdf_z_bound_, 1);

  /* calculate param */
  gmp_.resolution_inv_ = 1 / gmp_.resolution_;
  gmp_.map_origin_ = Eigen::Vector3d(-x_size / 2.0, -y_size / 2.0, gmp_.ground_height_);
  gmp_.map_size_ = Eigen::Vector3d(x_size, y_size, z_size);

  gmp_.prob_hit_log_ = logit(gmp_.p_hit_);
  gmp_.prob_miss_log_ = logit(gmp_.p_miss_);
  gmp_.clamp_min_log_ = logit(gmp_.p_min_);
  gmp_.clamp_max_log_ = logit(gmp_.p_max_);
  gmp_.min_occupancy_log_ = logit(gmp_.p_occ_);
  gmp_.unknown_flag_ = 0.01;

  cout << "[Gmap]: hit log   : " << gmp_.prob_hit_log_ << endl;
  cout << "[Gmap]: miss log  : " << gmp_.prob_miss_log_ << endl;
  cout << "[Gmap]: min log   : " << gmp_.clamp_min_log_ << endl;
  cout << "[Gmap]: max log   : " << gmp_.clamp_max_log_ << endl;
  cout << "[Gmap]: thresh log: " << gmp_.min_occupancy_log_ << endl;

  for (int i = 0; i < 3; ++i)
    gmp_.map_voxel_num_(i) = ceil(gmp_.map_size_(i) / gmp_.resolution_);

  gmp_.map_min_boundary_ = gmp_.map_origin_;
  gmp_.map_max_boundary_ = gmp_.map_origin_ + gmp_.map_size_;

  // initialize data buffers
  int buffer_size = gmp_.map_voxel_num_(0) * gmp_.map_voxel_num_(1) * gmp_.map_voxel_num_(2);

  gmd_.buffer_size_ = buffer_size;

  gmd_.occupancy_buffer_ = vector<double>(buffer_size, gmp_.clamp_min_log_ - gmp_.unknown_flag_);
  gmd_.occupancy_buffer_inflate_ = vector<int8_t>(buffer_size, -1);

  gmd_.count_hit_and_miss_ = vector<short>(buffer_size, 0);
  gmd_.count_hit_ = vector<short>(buffer_size, 0);
  gmd_.flag_rayend_ = vector<short>(buffer_size, -1);
  gmd_.flag_traverse_ = vector<short>(buffer_size, -1);

  gmd_.tmp_buffer1_ = vector<double>(gmd_.buffer_size_, 0);
  gmd_.tmp_buffer2_ = vector<double>(gmd_.buffer_size_, 0);
  gmd_.distance_buffer_ = vector<double>(gmd_.buffer_size_, 0);
  gmd_.distance_buffer_all_ = vector<double>(gmd_.buffer_size_, 0);
  gmd_.distance_buffer_neg_ = vector<double>(gmd_.buffer_size_, 0);
  gmd_.occupancy_buffer_neg = vector<int8_t>(gmd_.buffer_size_, 0);

  gmd_.raycast_num_ = 0;

  // TODO MZY 还真不一定是这个，如果输入变了的话
  gmd_.proj_pts_.resize((int)(ceil((640 - gmp_.depth_filter_margin_ * 2) / gmp_.skip_pixel_) *
                              ceil((480 - gmp_.depth_filter_margin_ * 2) / gmp_.skip_pixel_)));
  gmd_.proj_pts_cnt_ = 0;

  /* Callback and Timer*/
  depth_sub_.reset(new mfSubImg(node_, "grid_map/depth", 50));

  if (gmp_.pose_type_ == POSE_STAMPED)
  {
    pose_sub_.reset(
        new mfSubPose(node_, "grid_map/pose", 25, ros::TransportHints().tcpNoDelay()));
    sync_image_pose_.reset(
        new SynchronizerImagePose(SyncPolicyImagePose(100), *depth_sub_, *pose_sub_));
    sync_image_pose_->registerCallback(
        boost::bind(&GridMap::depthPoseCallback, this, _1, _2));
  }
  else if (gmp_.pose_type_ == ODOMETRY)
  {
    odom_sub_.reset(
        new mfSubOdom(node_, "grid_map/odom", 100, ros::TransportHints().tcpNoDelay()));
    sync_image_odom_.reset(
        new SynchronizerImageOdom(SyncPolicyImageOdom(100), *depth_sub_, *odom_sub_));
    sync_image_odom_->registerCallback(
        boost::bind(&GridMap::depthOdomCallback, this, _1, _2));
  }

  occ_timer_ = node_.createTimer(ros::Duration(0.05), &GridMap::updateOccupancyCallback, this);
  ESDF_timer_ = node_.createTimer(ros::Duration(0.1), &GridMap::updateESDFCallback, this);
  vis_timer_ = node_.createTimer(ros::Duration(0.2), &GridMap::visCallback, this);

  map_pub_ = node_.advertise<sensor_msgs::PointCloud2>("grid_map/occupancy", 10);
  map_inf_pub_ = node_.advertise<sensor_msgs::PointCloud2>("grid_map/occupancy_inflate", 10);
  map_esdf_pub_ = node_.advertise<sensor_msgs::PointCloud2>("grid_map/esdf", 10);

  gmd_.esdf_need_update_ = false;
  gmd_.occ_need_update_ = false;
  gmd_.local_updated_ = false;
  gmd_.has_first_depth_ = false;
  gmd_.has_odom_ = false;
  gmd_.has_cloud_ = false;
  gmd_.last_occ_update_time_.fromSec(0);

  gmd_.flag_depth_odom_timeout_ = false;
  gmd_.flag_use_depth_fusion = false;
}

int GridMap::setCacheOccupancy(Eigen::Vector3d pos, bool isOcc)
{
  Eigen::Vector3i id;
  posToIndex(pos, id);
  int idx_ctns = toAddress(id);

  // 该点被访问的次数，包括hit和miss
  gmd_.count_hit_and_miss_[idx_ctns] += 1;

  if (gmd_.count_hit_and_miss_[idx_ctns] == 1)
    gmd_.cache_voxel_.push(id);

  // 该点被hit的次数
  if (isOcc)
    gmd_.count_hit_[idx_ctns] += 1;

  return idx_ctns;
}

/**
 * Project depth image to points in world frame
 * Store in proj_pts_
 */
void GridMap::projectDepthImage()
{
  if (!gmd_.has_first_depth_)
  {
    gmd_.has_first_depth_ = true;
    return;
  }

  /* used for 32FC1 */
  // const double inv_factor = 1.0 / gmp_.k_depth_scaling_factor_;
  /* used for 16UC1 */
  constexpr double inv_factor = 0.001;
  uint16_t *row_ptr;
  int cols = gmd_.depth_image_.cols;
  int rows = gmd_.depth_image_.rows;
  Eigen::Matrix3d camera_r = gmd_.camera_r_m_;
  Eigen::Vector3d camera_p = gmd_.camera_pos_;
  const int skip_pix = gmp_.skip_pixel_;
  const int margin = gmp_.depth_filter_margin_;
  double depth;
  Eigen::Vector3d pt_cur, pt_world;
  gmd_.proj_pts_cnt_ = 0;

  /* filter margin of depth image */
  for (int v = margin; v < rows - margin; v += skip_pix)
  {
    row_ptr = gmd_.depth_image_.ptr<uint16_t>(v) + margin;
    for (int u = margin; u < cols - margin; u += skip_pix)
    {
      depth = (*row_ptr) * inv_factor;
      row_ptr = row_ptr + skip_pix;

      /* In case2, a special depth, will be processed in [raycastProcess] */
      if (depth < gmp_.depth_filter_mindist_) {
        continue;
      }
      else if (depth > gmp_.depth_filter_maxdist_) {
        depth = gmp_.max_ray_length_ + 0.1;
      }

      /* project to world frame */
      pt_cur(0) = (u - gmp_.cx_) * depth / gmp_.fx_;
      pt_cur(1) = (v - gmp_.cy_) * depth / gmp_.fy_;
      pt_cur(2) = depth;
      pt_world = camera_r * pt_cur + camera_p;
      gmd_.proj_pts_[gmd_.proj_pts_cnt_++] = pt_world;
    }
  }
}

void GridMap::raycastProcess()
{
  if (gmd_.proj_pts_cnt_ == 0)
    return;

  gmd_.raycast_num_ += 1;

  int vox_idx;
  double length;
  Eigen::Vector3d camera_p = gmd_.camera_pos_;
  RayCaster raycaster;
  Eigen::Vector3d half = Eigen::Vector3d(0.5, 0.5, 0.5);
  Eigen::Vector3d ray_pt, pt_w;

  // bounding box of updated region
  double min_x = min(gmp_.map_max_boundary_(0), camera_p(0));
  double min_y = min(gmp_.map_max_boundary_(1), camera_p(1));
  double min_z = min(gmp_.map_max_boundary_(2), camera_p(2));

  double max_x = max(gmp_.map_min_boundary_(0), camera_p(0));
  double max_y = max(gmp_.map_min_boundary_(1), camera_p(1));
  double max_z = max(gmp_.map_min_boundary_(2), camera_p(2));
  max_z = max(max_z, gmp_.ground_height_);

  for (int i = 0; i < gmd_.proj_pts_cnt_; ++i)
  {
    pt_w = gmd_.proj_pts_[i];
    if (!isInMap(pt_w))
    {
      pt_w = closetPointInMap(pt_w, camera_p);
      length = (pt_w - camera_p).norm();
      if (length > gmp_.max_ray_length_)
      {
        pt_w = (pt_w - camera_p) / length * gmp_.max_ray_length_ + camera_p;
      }
      vox_idx = setCacheOccupancy(pt_w, false);
    }
    else
    {
      length = (pt_w - camera_p).norm();
      if (length > gmp_.max_ray_length_)
      {
        pt_w = (pt_w - camera_p) / length * gmp_.max_ray_length_ + camera_p;
        vox_idx = setCacheOccupancy(pt_w, false);
      }
      else
      {
        vox_idx = setCacheOccupancy(pt_w, true);
      }
    }

    max_x = max(max_x, pt_w(0));
    max_y = max(max_y, pt_w(1));
    max_z = max(max_z, pt_w(2));
    min_x = min(min_x, pt_w(0));
    min_y = min(min_y, pt_w(1));
    min_z = min(min_z, pt_w(2));

    /**
     * Avoid raycast the points which have the same id
     * @note after test, the redundant points been cut off
     *       account for 95% of the total points
     */
    if (gmd_.flag_rayend_[vox_idx] == gmd_.raycast_num_)
      continue;
    else
      gmd_.flag_rayend_[vox_idx] = gmd_.raycast_num_;

    /* Raycasting between camera center and point */
    raycaster.setInput(pt_w * gmp_.resolution_inv_, camera_p * gmp_.resolution_inv_);
    while (raycaster.step(ray_pt))
    {
      Eigen::Vector3d tmp = (ray_pt + half) * gmp_.resolution_;
      vox_idx = setCacheOccupancy(tmp, false);

      /**
       * @note after test, the redundant points been cut off
       *       account for 20% of the total points
       */
      if (gmd_.flag_traverse_[vox_idx] == gmd_.raycast_num_)
        break;
      else
        gmd_.flag_traverse_[vox_idx] = gmd_.raycast_num_;
    }
  }

  /* find a box bound of sensor range */
  posToIndex(Eigen::Vector3d(max_x, max_y, max_z), gmd_.local_bound_max_);
  posToIndex(Eigen::Vector3d(min_x, min_y, min_z), gmd_.local_bound_min_);
  boundIndex(gmd_.local_bound_min_);
  boundIndex(gmd_.local_bound_max_);

  gmd_.local_updated_ = true;

  /* update occupancy cached in queue */
  Eigen::Vector3d local_range_min = camera_p - gmp_.local_update_range_;
  Eigen::Vector3d local_range_max = camera_p + gmp_.local_update_range_;
  Eigen::Vector3i min_id, max_id;
  posToIndex(local_range_min, min_id);
  posToIndex(local_range_max, max_id);
  boundIndex(min_id);
  boundIndex(max_id);
  while (!gmd_.cache_voxel_.empty())
  {
    Eigen::Vector3i idx = gmd_.cache_voxel_.front();
    int idx_ctns = toAddress(idx);
    gmd_.cache_voxel_.pop();

    gmd_.occupancy_buffer_inflate_[idx_ctns] = 0;
    // gmd_.occupancy_buffer_test_[idx_ctns] = 0;

    bool ishit = gmd_.count_hit_[idx_ctns] >=
                 gmd_.count_hit_and_miss_[idx_ctns] - gmd_.count_hit_[idx_ctns];
    double log_odds_update = ishit ? gmp_.prob_hit_log_ : gmp_.prob_miss_log_;

    gmd_.count_hit_[idx_ctns] = gmd_.count_hit_and_miss_[idx_ctns] = 0;

    // TODO 如果要考虑动态障碍物，这一块需要再想想
    if (log_odds_update >= 0 &&
        gmd_.occupancy_buffer_[idx_ctns] >= gmp_.clamp_max_log_)
    {
      continue;
    }
    else if (log_odds_update <= 0 &&
             gmd_.occupancy_buffer_[idx_ctns] <= gmp_.clamp_min_log_)
    {
      gmd_.occupancy_buffer_[idx_ctns] = gmp_.clamp_min_log_;
      continue;
    }

    // TODO MZY 个人认为这个判断是错的，not in local更加应该相信之前的数据吧
    //          所以应该将这一次更新的设置为0?
    // 20231224: 所以他这里是为了处理动态障碍物？
    //           但也不对阿，他看不到有了上面 depth_filter_maxdist_和
    //           max_ray_length_的限制，他也看不到local之外的部分阿.
    //           经过测试，in_local一直是true的!!
    /* if point not in local sensor range, set the previous to min */
    bool in_local = idx(0) >= min_id(0) && idx(0) <= max_id(0) &&
                    idx(1) >= min_id(1) && idx(1) <= max_id(1) &&
                    idx(2) >= min_id(2) && idx(2) <= max_id(2);
    if (!in_local)
    {
      gmd_.occupancy_buffer_[idx_ctns] = gmp_.clamp_min_log_;
    }

    gmd_.occupancy_buffer_[idx_ctns] =
        std::min(
            std::max(gmd_.occupancy_buffer_[idx_ctns] + log_odds_update,
                     gmp_.clamp_min_log_),
            gmp_.clamp_max_log_);
  }
}

Eigen::Vector3d GridMap::closetPointInMap(const Eigen::Vector3d &pt,
                                          const Eigen::Vector3d &camera_pt)
{
  Eigen::Vector3d diff = pt - camera_pt;
  Eigen::Vector3d max_tc = gmp_.map_max_boundary_ - camera_pt;
  Eigen::Vector3d min_tc = gmp_.map_min_boundary_ - camera_pt;

  double min_t = std::numeric_limits<double>::max();
  for (int i = 0; i < 3; ++i)
  {
    if (fabs(diff[i]) > 0)
    {
      double t1 = max_tc[i] / diff[i];
      if (t1 > 0 && t1 < min_t)
        min_t = t1;

      double t2 = min_tc[i] / diff[i];
      if (t2 > 0 && t2 < min_t)
        min_t = t2;
    }
  }
  return camera_pt + (min_t - 1e-2) * diff;
}

Eigen::Vector2d GridMap::closetPointInMap(const Eigen::Vector2d &pt,
                                          const Eigen::Vector2d &camera_pt)
{
  Eigen::Vector2d diff = pt - camera_pt;
  Eigen::Vector2d max_tc = gmp_.map_max_boundary_.head(2) - camera_pt;
  Eigen::Vector2d min_tc = gmp_.map_min_boundary_.head(2) - camera_pt;

  double min_t = std::numeric_limits<double>::max();
  for (int i = 0; i < 2; ++i)
  {
    if (fabs(diff[i]) > 0)
    {
      double t1 = max_tc[i] / diff[i];
      if (t1 > 0 && t1 < min_t)
        min_t = t1;

      double t2 = min_tc[i] / diff[i];
      if (t2 > 0 && t2 < min_t)
        min_t = t2;
    }
  }
  return camera_pt + (min_t - 1e-2) * diff;
}

void GridMap::clearAndInflateLocalMap()
{
  Eigen::Vector3i local_min = gmd_.local_bound_min_;
  Eigen::Vector3i local_max = gmd_.local_bound_max_;

  // TODO: clear data outside the local range
  // TODO  貌似是对于动态场景的
  // const int vec_margin = 5;
  // Eigen::Vector3i min_cut = gmd_.local_bound_min_ -
  //                           Eigen::Vector3i(gmp_.local_map_margin_,
  //                                           gmp_.local_map_margin_,
  //                                           gmp_.local_map_margin_);
  // Eigen::Vector3i max_cut = gmd_.local_bound_max_ +
  //                           Eigen::Vector3i(gmp_.local_map_margin_,
  //                                           gmp_.local_map_margin_,
  //                                           gmp_.local_map_margin_);
  // boundIndex(min_cut);
  // boundIndex(max_cut);
  // Eigen::Vector3i min_cut_m = min_cut -
  //                             Eigen::Vector3i(vec_margin, vec_margin, vec_margin);
  // Eigen::Vector3i max_cut_m = max_cut +
  //                             Eigen::Vector3i(vec_margin, vec_margin, vec_margin);
  // boundIndex(min_cut_m);
  // boundIndex(max_cut_m);

  // for (int x = min_cut_m(0); x <= max_cut_m(0); ++x)
  //   for (int y = min_cut_m(1); y <= max_cut_m(1); ++y)
  //   {
  //     for (int z = min_cut_m(2); z < min_cut(2); ++z)
  //     {
  //       int idx = toAddress(x, y, z);
  //       gmd_.occupancy_buffer_[idx] = gmp_.clamp_min_log_ - gmp_.unknown_flag_;
  //     }

  //     for (int z = max_cut(2) + 1; z <= max_cut_m(2); ++z)
  //     {
  //       int idx = toAddress(x, y, z);
  //       gmd_.occupancy_buffer_[idx] = gmp_.clamp_min_log_ - gmp_.unknown_flag_;
  //     }
  //   }

  // for (int z = min_cut_m(2); z <= max_cut_m(2); ++z)
  //   for (int x = min_cut_m(0); x <= max_cut_m(0); ++x)
  //   {
  //     for (int y = min_cut_m(1); y < min_cut(1); ++y)
  //     {
  //       int idx = toAddress(x, y, z);
  //       gmd_.occupancy_buffer_[idx] = gmp_.clamp_min_log_ - gmp_.unknown_flag_;
  //     }

  //     for (int y = max_cut(1) + 1; y <= max_cut_m(1); ++y)
  //     {
  //       int idx = toAddress(x, y, z);
  //       gmd_.occupancy_buffer_[idx] = gmp_.clamp_min_log_ - gmp_.unknown_flag_;
  //     }
  //   }

  // for (int y = min_cut_m(1); y <= max_cut_m(1); ++y)
  //   for (int z = min_cut_m(2); z <= max_cut_m(2); ++z)
  //   {
  //     for (int x = min_cut_m(0); x < min_cut(0); ++x)
  //     {
  //       int idx = toAddress(x, y, z);
  //       gmd_.occupancy_buffer_[idx] = gmp_.clamp_min_log_ - gmp_.unknown_flag_;
  //     }

  //     for (int x = max_cut(0) + 1; x <= max_cut_m(0); ++x)
  //     {
  //       int idx = toAddress(x, y, z);
  //       gmd_.occupancy_buffer_[idx] = gmp_.clamp_min_log_ - gmp_.unknown_flag_;
  //     }
  //   }

  // TODO mzy 这个要根据飞机尺寸来的吧，感觉有点小了
  /* Inflate occupied voxels to compensate robot size */
  int inf_step = ceil(gmp_.obstacles_inflation_ * gmp_.resolution_inv_);
  vector<Eigen::Vector3i> inf_pts(pow(2 * inf_step + 1, 3));

  // inflate obstacles
  for (int x = local_min(0); x <= local_max(0); ++x)
    for (int y = local_min(1); y <= local_max(1); ++y)
      for (int z = local_min(2); z <= local_max(2); ++z)
      {
        // clear outdated data
        int address = toAddress(x, y, z);

        if (gmd_.occupancy_buffer_[address] > gmp_.min_occupancy_log_)
        {
          inflatePoint(Eigen::Vector3i(x, y, z), inf_step, inf_pts);
          for (size_t k = 0; k < inf_pts.size(); ++k)
          {
            if (!isInMap(inf_pts[k]))
              continue;

            int idx_inf = toAddress(inf_pts[k]);
            gmd_.occupancy_buffer_inflate_[idx_inf] = 1;
          }
        }
      }

  // TODO MZY 实机飞的时候要注意
  // add virtual ceiling to limit flight height
  if (gmp_.virtual_ceil_height_ > -0.5)
  {
    int ceil_id = floor((gmp_.virtual_ceil_height_ - gmp_.map_origin_(2)) *
                        gmp_.resolution_inv_);
    for (int x = local_min(0); x <= local_max(0); ++x)
      for (int y = local_min(1); y <= local_max(1); ++y)
      {
        gmd_.occupancy_buffer_inflate_[toAddress(x, y, ceil_id)] = 1;
      }
  }
}

// ref https://blog.csdn.net/tugepaopaoo/article/details/130167750
// ref Pedro F. Felzenszwalb, Huttenlocher Daniel P.
//     Distance Transforms of Sampled Functions[J].
//     Theory of Computing, 2012, 8(1): 415-428
// 再看这一段还是会觉得很精妙
template <typename F_get_val, typename F_set_val>
void GridMap::fillESDF(F_get_val f_get_val, F_set_val f_set_val,
                       int start, int end, int dim)
{
  int v[gmp_.map_voxel_num_(dim)];
  double z[gmp_.map_voxel_num_(dim) + 1];

  int k = start;
  v[start] = start;
  z[start] = -std::numeric_limits<double>::max();
  z[start + 1] = std::numeric_limits<double>::max();

  for (int q = start + 1; q <= end; q++)
  {
    k++;
    double s;

    do
    {
      k--;
      s = ((f_get_val(q) + q * q) - (f_get_val(v[k]) + v[k] * v[k])) /
          (2 * q - 2 * v[k]);
    } while (s <= z[k]);

    k++;

    v[k] = q;
    z[k] = s;
    z[k + 1] = std::numeric_limits<double>::max();
  }

  k = start;

  for (int q = start; q <= end; q++)
  {
    while (z[k + 1] < q)
      k++;
    double val = (q - v[k]) * (q - v[k]) + f_get_val(v[k]);
    f_set_val(q, val);
  }
}

void GridMap::updateESDF3d()
{
  //* 先确定update的一个范围
  int x_bound = gmp_.esdf_x_bound_;
  int y_bound = gmp_.esdf_y_bound_;
  int z_bound = gmp_.esdf_z_bound_;

  Eigen::Vector3i odom_index;
  Eigen::Vector3d odom_pos = gmd_.camera_pos_;
  posToIndex(odom_pos, odom_index);
  Eigen::Vector3i esdf_bound{x_bound, y_bound, z_bound};

  Eigen::Vector3i min_esdf = gmd_.min_esdf_;
  Eigen::Vector3i max_esdf = gmd_.max_esdf_;

  /* ========== clear ESDF ========== */
  for (int x = min_esdf[0]; x <= max_esdf[0]; x++)
  {
    for (int y = min_esdf[1]; y <= max_esdf[1]; y++)
    {
      for (int z = min_esdf[2]; z <= max_esdf[2]; z++)
      {
        int idr = toAddress(x, y, z);
        gmd_.tmp_buffer1_[idr] = 0;
        gmd_.tmp_buffer2_[idr] = 0;
        gmd_.distance_buffer_[idr] = 0;
        gmd_.distance_buffer_all_[idr] = 0;
        gmd_.distance_buffer_neg_[idr] = 0;
        gmd_.occupancy_buffer_neg[idr] = 0;
      }
    }
  }

  gmd_.min_esdf_ = odom_index - esdf_bound;
  gmd_.max_esdf_ = odom_index + esdf_bound;

  boundIndex(gmd_.min_esdf_);
  boundIndex(gmd_.max_esdf_);

  min_esdf = gmd_.min_esdf_;
  max_esdf = gmd_.max_esdf_;

  /* ========== compute positive DT ========== */

  for (int x = min_esdf[0]; x <= max_esdf[0]; x++)
  {
    for (int y = min_esdf[1]; y <= max_esdf[1]; y++)
    {
      fillESDF(
          [&](int z) {
            return (gmd_.occupancy_buffer_inflate_[toAddress(x, y, z)] == 1)
                       ? 0
                       : std::numeric_limits<double>::max();
          },
          [&](int z, double val) { gmd_.tmp_buffer1_[toAddress(x, y, z)] = val; },
          min_esdf[2],
          max_esdf[2], 2);
    }
  }

  for (int x = min_esdf[0]; x <= max_esdf[0]; x++)
  {
    for (int z = min_esdf[2]; z <= max_esdf[2]; z++)
    {
      fillESDF([&](int y) { return gmd_.tmp_buffer1_[toAddress(x, y, z)]; },
               [&](int y, double val) { gmd_.tmp_buffer2_[toAddress(x, y, z)] = val; },
               min_esdf[1],
               max_esdf[1], 1);
    }
  }

  for (int y = min_esdf[1]; y <= max_esdf[1]; y++)
  {
    for (int z = min_esdf[2]; z <= max_esdf[2]; z++)
    {
      fillESDF([&](int x) { return gmd_.tmp_buffer2_[toAddress(x, y, z)]; },
               [&](int x, double val) {
                 gmd_.distance_buffer_[toAddress(x, y, z)] =
                     gmp_.resolution_ * std::sqrt(val);
               },
               min_esdf[0], max_esdf[0], 0);
    }
  }

  /* ========== compute negative distance ========== */
  for (int x = min_esdf(0); x <= max_esdf(0); ++x)
    for (int y = min_esdf(1); y <= max_esdf(1); ++y)
      for (int z = min_esdf(2); z <= max_esdf(2); ++z)
      {

        int idx = toAddress(x, y, z);
        int8_t occ = gmd_.occupancy_buffer_inflate_[idx];
        // TODO 这一块， ==-1其实是未知情况
        if (occ == 0 || occ == -1)
        {
          gmd_.occupancy_buffer_neg[idx] = 1;
        }
        else if ((gmd_.occupancy_buffer_inflate_[idx] == 1))
        {
          gmd_.occupancy_buffer_neg[idx] = 0;
        }
        else
        {
          std::cout << "occ=" << occ << std::endl;
          ROS_ERROR("what?");
        }
      }

  for (int x = min_esdf[0]; x <= max_esdf[0]; x++)
  {
    for (int y = min_esdf[1]; y <= max_esdf[1]; y++)
    {
      fillESDF(
          [&](int z) {
            return gmd_.occupancy_buffer_neg[x * gmp_.map_voxel_num_(1) *
                                                 gmp_.map_voxel_num_(2) +
                                             y * gmp_.map_voxel_num_(2) +
                                             z] == 1
                       ? 0
                       : std::numeric_limits<double>::max();
          },
          [&](int z, double val) { gmd_.tmp_buffer1_[toAddress(x, y, z)] = val; },
          min_esdf[2],
          max_esdf[2], 2);
    }
  }

  for (int x = min_esdf[0]; x <= max_esdf[0]; x++)
  {
    for (int z = min_esdf[2]; z <= max_esdf[2]; z++)
    {
      fillESDF([&](int y) { return gmd_.tmp_buffer1_[toAddress(x, y, z)]; },
               [&](int y, double val) { gmd_.tmp_buffer2_[toAddress(x, y, z)] = val; },
               min_esdf[1],
               max_esdf[1], 1);
    }
  }

  for (int y = min_esdf[1]; y <= max_esdf[1]; y++)
  {
    for (int z = min_esdf[2]; z <= max_esdf[2]; z++)
    {
      fillESDF([&](int x) { return gmd_.tmp_buffer2_[toAddress(x, y, z)]; },
               [&](int x, double val) {
                 gmd_.distance_buffer_neg_[toAddress(x, y, z)] =
                     gmp_.resolution_ * std::sqrt(val);
               },
               min_esdf[0], max_esdf[0], 0);
    }
  }

  /* ========== combine pos and neg DT ========== */
  for (int x = min_esdf(0); x <= max_esdf(0); ++x)
    for (int y = min_esdf(1); y <= max_esdf(1); ++y)
      for (int z = min_esdf(2); z <= max_esdf(2); ++z)
      {

        int idx = toAddress(x, y, z);
        gmd_.distance_buffer_all_[idx] = gmd_.distance_buffer_[idx];

        if (gmd_.distance_buffer_neg_[idx] > 0.0)
          gmd_.distance_buffer_all_[idx] +=
              (-gmd_.distance_buffer_neg_[idx] + gmp_.resolution_);
      }
}

void GridMap::updateESDFCallback(const ros::TimerEvent & /*event*/)
{
  if (!gmd_.esdf_need_update_)
    return;

  // ros::Time t1, t2;
  // t1 = ros::Time::now();
  updateESDF3d();
  // t2 = ros::Time::now();
  // ROS_WARN("ESDF: cur t = %lf, avg t = %lf, max t = %lf", (t2 - t1).toSec());

  gmd_.esdf_need_update_ = false;
}

void GridMap::visCallback(const ros::TimerEvent & /*event*/)
{
  if (!gmd_.has_odom_)
    return;

  publishMapInflate(true);
  publishMap();
  publishESDF();
}

void GridMap::updateOccupancyCallback(const ros::TimerEvent & /*event*/)
{
  if (gmd_.last_occ_update_time_.toSec() < 1.0)
    gmd_.last_occ_update_time_ = ros::Time::now();

  if (!gmd_.occ_need_update_)
  {
    if (gmd_.flag_use_depth_fusion &&
        (ros::Time::now() - gmd_.last_occ_update_time_).toSec() >
            gmp_.odom_depth_timeout_)
    {
      ROS_ERROR("depth lost! t_now=%f, t_last_update_occ=%f, timeout_thres=%f",
                ros::Time::now().toSec(), gmd_.last_occ_update_time_.toSec(),
                gmp_.odom_depth_timeout_);
      gmd_.flag_depth_odom_timeout_ = true;
    }
    return;
  }
  gmd_.last_occ_update_time_ = ros::Time::now();

  // ros::Time t1, t2, t3, t4;
  // t1 = ros::Time::now();
  projectDepthImage();
  // t2 = ros::Time::now();
  raycastProcess();
  // t3 = ros::Time::now();

  if (gmd_.local_updated_)
    clearAndInflateLocalMap();

  // t4 = ros::Time::now();

  // cout << setprecision(7);
  // cout << "t2=" << (t2 - t1).toSec()
  //      << " t3=" << (t3 - t2).toSec() << " t4=" << (t4 - t3).toSec() << endl;

  gmd_.esdf_need_update_ = true;
  gmd_.occ_need_update_ = false;
  gmd_.local_updated_ = false;
}

void GridMap::depthPoseCallback(const sensor_msgs::ImageConstPtr &img,
                                const geometry_msgs::PoseStampedConstPtr &pose)
{
  /* get depth image */
  cv_bridge::CvImagePtr cv_ptr;
  cv_ptr = cv_bridge::toCvCopy(img, img->encoding);

  /* used for TYPE_32FC1*/
  // if (img->encoding == sensor_msgs::image_encodings::TYPE_32FC1)
  // {
  //   (cv_ptr->image).convertTo(cv_ptr->image, CV_16UC1, gmp_.k_depth_scaling_factor_);
  // }
  cv_ptr->image.copyTo(gmd_.depth_image_);

  /* get pose */
  gmd_.camera_pos_(0) = pose->pose.position.x;
  gmd_.camera_pos_(1) = pose->pose.position.y;
  gmd_.camera_pos_(2) = pose->pose.position.z;
  gmd_.camera_r_m_ = Eigen::Quaterniond(pose->pose.orientation.w,
                                        pose->pose.orientation.x,
                                        pose->pose.orientation.y,
                                        pose->pose.orientation.z)
                         .toRotationMatrix();

  if (isInMap(gmd_.camera_pos_))
  {
    gmd_.has_odom_ = true;
    gmd_.occ_need_update_ = true;
  }
  else
  {
    gmd_.occ_need_update_ = false;
  }
  gmd_.flag_use_depth_fusion = true;
}

void GridMap::depthOdomCallback(const sensor_msgs::ImageConstPtr &img,
                                const nav_msgs::OdometryConstPtr &odom)
{
  /* get depth image */
  cv_bridge::CvImagePtr cv_ptr;
  cv_ptr = cv_bridge::toCvCopy(img, img->encoding);

  /* used for TYPE_32FC1*/
  // if (img->encoding == sensor_msgs::image_encodings::TYPE_32FC1)
  // {
  //   (cv_ptr->image).convertTo(cv_ptr->image, CV_16UC1, gmp_.k_depth_scaling_factor_);
  // }
  cv_ptr->image.copyTo(gmd_.depth_image_);

  /* get pose */
  gmd_.camera_pos_(0) = odom->pose.pose.position.x;
  gmd_.camera_pos_(1) = odom->pose.pose.position.y;
  gmd_.camera_pos_(2) = odom->pose.pose.position.z;
  gmd_.camera_r_m_ = Eigen::Quaterniond(odom->pose.pose.orientation.w,
                                        odom->pose.pose.orientation.x,
                                        odom->pose.pose.orientation.y,
                                        odom->pose.pose.orientation.z)
                         .toRotationMatrix();

  if (isInMap(gmd_.camera_pos_))
  {
    gmd_.has_odom_ = true;
    gmd_.occ_need_update_ = true;
  }
  else
  {
    gmd_.occ_need_update_ = false;
  }
  gmd_.flag_use_depth_fusion = true;
}

/**
 * TODO MZY 先留着，如果之后的输入是点云的话也可以使用
 */
// void GridMap::cloudCallback(const sensor_msgs::PointCloud2ConstPtr &img)
// {
//   pcl::PointCloud<pcl::PointXYZ> latest_cloud;
//   pcl::fromROSMsg(*img, latest_cloud);

//   gmd_.has_cloud_ = true;

//   if (!gmd_.has_odom_)
//   {
//     std::cout << "no odom!" << std::endl;
//     return;
//   }

//   if (latest_cloud.points.size() == 0)
//     return;

//   if (isnan(gmd_.camera_pos_(0)) || isnan(gmd_.camera_pos_(1)) || isnan(gmd_.camera_pos_(2)))
//     return;

//   /* reset occ buffer */
//   Eigen::Vector3i min_id, max_id;
//   posToIndex(gmd_.camera_pos_ - gmp_.local_update_range_, min_id);
//   posToIndex(gmd_.camera_pos_ + gmp_.local_update_range_, max_id);
//   boundIndex(min_id);
//   boundIndex(max_id);
//   for (int x = min_id(0); x <= max_id(0); ++x)
//     for (int y = min_id(1); y <= max_id(1); ++y)
//       for (int z = min_id(2); z <= max_id(2); ++z)
//       {
//         gmd_.occupancy_buffer_inflate_[toAddress(x, y, z)] = 0;
//       }

//   pcl::PointXYZ pt;
//   Eigen::Vector3d p3d, p3d_inf;

//   int inf_step = ceil(gmp_.obstacles_inflation_ / gmp_.resolution_);
//   int inf_step_z = 1;

//   double max_x, max_y, max_z, min_x, min_y, min_z;

//   min_x = gmp_.map_max_boundary_(0);
//   min_y = gmp_.map_max_boundary_(1);
//   min_z = gmp_.map_max_boundary_(2);

//   max_x = gmp_.map_min_boundary_(0);
//   max_y = gmp_.map_min_boundary_(1);
//   max_z = gmp_.map_min_boundary_(2);

//   for (size_t i = 0; i < latest_cloud.points.size(); ++i)
//   {
//     pt = latest_cloud.points[i];
//     p3d(0) = pt.x, p3d(1) = pt.y, p3d(2) = pt.z;

//     /* point inside update range */
//     Eigen::Vector3d devi = p3d - gmd_.camera_pos_;
//     Eigen::Vector3i inf_pt;

//     if (fabs(devi(0)) < gmp_.local_update_range_(0) && fabs(devi(1)) < gmp_.local_update_range_(1) &&
//         fabs(devi(2)) < gmp_.local_update_range_(2))
//     {

//       /* inflate the point */
//       for (int x = -inf_step; x <= inf_step; ++x)
//         for (int y = -inf_step; y <= inf_step; ++y)
//           for (int z = -inf_step_z; z <= inf_step_z; ++z)
//           {

//             p3d_inf(0) = pt.x + x * gmp_.resolution_;
//             p3d_inf(1) = pt.y + y * gmp_.resolution_;
//             p3d_inf(2) = pt.z + z * gmp_.resolution_;

//             max_x = max(max_x, p3d_inf(0));
//             max_y = max(max_y, p3d_inf(1));
//             max_z = max(max_z, p3d_inf(2));

//             min_x = min(min_x, p3d_inf(0));
//             min_y = min(min_y, p3d_inf(1));
//             min_z = min(min_z, p3d_inf(2));

//             posToIndex(p3d_inf, inf_pt);

//             if (!isInMap(inf_pt))
//               continue;

//             int idx_inf = toAddress(inf_pt);

//             gmd_.occupancy_buffer_inflate_[idx_inf] = 1;
//           }
//     }
//   }

//   min_x = min(min_x, gmd_.camera_pos_(0));
//   min_y = min(min_y, gmd_.camera_pos_(1));
//   min_z = min(min_z, gmd_.camera_pos_(2));
//   max_x = max(max_x, gmd_.camera_pos_(0));
//   max_y = max(max_y, gmd_.camera_pos_(1));
//   max_z = max(max_z, gmd_.camera_pos_(2));

//   max_z = max(max_z, gmp_.ground_height_);

//   posToIndex(Eigen::Vector3d(max_x, max_y, max_z), gmd_.local_bound_max_);
//   posToIndex(Eigen::Vector3d(min_x, min_y, min_z), gmd_.local_bound_min_);

//   boundIndex(gmd_.local_bound_min_);
//   boundIndex(gmd_.local_bound_max_);

//   // add virtual ceiling to limit flight height
//   if (gmp_.virtual_ceil_height_ > -0.5)
//   {
//     int ceil_id = floor((gmp_.virtual_ceil_height_ - gmp_.map_origin_(2)) * gmp_.resolution_inv_);
//     for (int x = gmd_.local_bound_min_(0); x <= gmd_.local_bound_max_(0); ++x)
//       for (int y = gmd_.local_bound_min_(1); y <= gmd_.local_bound_max_(1); ++y)
//       {
//         gmd_.occupancy_buffer_inflate_[toAddress(x, y, ceil_id)] = 1;
//       }
//   }
// }

void GridMap::publishMap()
{

  if (map_pub_.getNumSubscribers() <= 0)
    return;

  pcl::PointXYZ pt;
  pcl::PointCloud<pcl::PointXYZ> cloud;

  Eigen::Vector3i min_cut = gmd_.local_bound_min_;
  Eigen::Vector3i max_cut = gmd_.local_bound_max_;

  int lmm = gmp_.local_map_margin_ / 2;
  min_cut -= Eigen::Vector3i(lmm, lmm, lmm);
  max_cut += Eigen::Vector3i(lmm, lmm, lmm);

  // TODO TEST
  min_cut -= Eigen::Vector3i(40, 40, 0);
  max_cut += Eigen::Vector3i(40, 40, 0);

  boundIndex(min_cut);
  boundIndex(max_cut);

  for (int x = min_cut(0); x <= max_cut(0); ++x)
    for (int y = min_cut(1); y <= max_cut(1); ++y)
      for (int z = min_cut(2); z <= max_cut(2); ++z)
      {
        if (gmd_.occupancy_buffer_[toAddress(x, y, z)] <
            gmp_.min_occupancy_log_)
          continue;

        Eigen::Vector3d pos;
        indexToPos(Eigen::Vector3i(x, y, z), pos);
        if (pos(2) > gmp_.visual_truncate_height_)
          continue;

        pt.x = pos(0);
        pt.y = pos(1);
        pt.z = pos(2);
        cloud.push_back(pt);
      }

  cloud.width = cloud.points.size();
  cloud.height = 1;
  cloud.is_dense = true;
  cloud.header.frame_id = gmp_.frame_id_;
  sensor_msgs::PointCloud2 cloud_msg;

  pcl::toROSMsg(cloud, cloud_msg);
  map_pub_.publish(cloud_msg);
}

void GridMap::publishESDF()
{
  if (map_esdf_pub_.getNumSubscribers() <= 0)
    return;

  int x_bound = gmp_.esdf_x_bound_;
  int y_bound = gmp_.esdf_y_bound_;

  pcl::PointXYZI pt;
  pcl::PointCloud<pcl::PointXYZI> cloud_esdf;

  Eigen::Vector3i odom_index;
  Eigen::Vector3d odom_pos = gmd_.camera_pos_;
  posToIndex(odom_pos, odom_index);

  const double min_dist = -3.0;
  const double max_dist = 3.0;
  double dist;

  int z = odom_index(2);
  for (int x = odom_index(0) - x_bound; x <= odom_index(0) + x_bound; ++x)
    for (int y = odom_index(1) - y_bound; y <= odom_index(1) + y_bound; ++y)
    {
      Eigen::Vector3i id{x, y, z};
      if (!isInMap(id))
        continue;
      int adr = toAddress(id);

      dist = gmd_.distance_buffer_all_[adr];
      dist = max(min(dist, max_dist), min_dist);

      Eigen::Vector3d pos;
      indexToPos(Eigen::Vector3i(x, y, z), pos);

      pt.x = pos(0);
      pt.y = pos(1);
      pt.z = -0.2;
      pt.intensity = (dist - min_dist) / (max_dist - min_dist);
      cloud_esdf.push_back(pt);
    }

  cloud_esdf.width = cloud_esdf.points.size();
  cloud_esdf.height = 1;
  cloud_esdf.is_dense = true;
  cloud_esdf.header.frame_id = gmp_.frame_id_;
  sensor_msgs::PointCloud2 cloud_msg;

  pcl::toROSMsg(cloud_esdf, cloud_msg);
  map_esdf_pub_.publish(cloud_msg);
}

void GridMap::publishMapInflate(bool all_info)
{
  if (map_inf_pub_.getNumSubscribers() <= 0)
    return;

  pcl::PointXYZ pt;
  pcl::PointCloud<pcl::PointXYZ> cloud;

  Eigen::Vector3i min_cut = gmd_.local_bound_min_;
  Eigen::Vector3i max_cut = gmd_.local_bound_max_;

  if (all_info)
  {
    int lmm = gmp_.local_map_margin_;
    min_cut -= Eigen::Vector3i(lmm, lmm, lmm);
    max_cut += Eigen::Vector3i(lmm, lmm, lmm);
  }

  // TODO TEST
  min_cut -= Eigen::Vector3i(40, 40, 0);
  max_cut += Eigen::Vector3i(40, 40, 0);
  boundIndex(min_cut);
  boundIndex(max_cut);

  for (int x = min_cut(0); x <= max_cut(0); ++x)
    for (int y = min_cut(1); y <= max_cut(1); ++y)
      for (int z = min_cut(2); z <= max_cut(2); ++z)
      {
        int8_t occ = gmd_.occupancy_buffer_inflate_[toAddress(x, y, z)];
        if (occ == 0 || occ == -1)
          continue;

        Eigen::Vector3d pos;
        indexToPos(Eigen::Vector3i(x, y, z), pos);
        if (pos(2) > gmp_.visual_truncate_height_)
          continue;

        pt.x = pos(0);
        pt.y = pos(1);
        pt.z = pos(2);
        cloud.push_back(pt);
      }

  cloud.width = cloud.points.size();
  cloud.height = 1;
  cloud.is_dense = true;
  cloud.header.frame_id = gmp_.frame_id_;
  sensor_msgs::PointCloud2 cloud_msg;

  pcl::toROSMsg(cloud, cloud_msg);
  map_inf_pub_.publish(cloud_msg);
}

bool GridMap::evaluateESDF(const Eigen::Vector3d &pos,
                           double &dist)
{
  if (!isInESDF(pos))
    return false;

  Eigen::Vector3d diff;
  Eigen::Vector3d sur_pts[2][2][2];
  getSurroundPts(pos, sur_pts, diff);

  double dists[2][2][2];
  getSurroundDistance(sur_pts, dists);
  interpolateTrilinear(dists, diff, dist);

  return true;
}

bool GridMap::evaluateESDFWithGrad(const Eigen::Vector3d &pos,
                                   double &dist,
                                   Eigen::Vector3d &grad)
{
  if (!isInESDF(pos))
    return false;

  Eigen::Vector3d diff;
  Eigen::Vector3d sur_pts[2][2][2];
  getSurroundPts(pos, sur_pts, diff);

  double dists[2][2][2];
  getSurroundDistance(sur_pts, dists);
  interpolateTrilinear(dists, diff, dist, grad);

  return true;
}

/**
 * @brief get the pts around [pos] and the bias between [pos] and
 *        the corresponding pt in the grid map
 */
void GridMap::getSurroundPts(const Eigen::Vector3d &pos,
                             Eigen::Vector3d pts[2][2][2],
                             Eigen::Vector3d &diff)
{
  /* interpolation position */
  Eigen::Vector3d pos_m = pos -
                          0.5 * gmp_.resolution_ * Eigen::Vector3d::Ones();
  Eigen::Vector3i idx;
  Eigen::Vector3d idx_pos;

  posToIndex(pos_m, idx);
  indexToPos(idx, idx_pos);
  diff = (pos - idx_pos) * gmp_.resolution_inv_;

  for (int x = 0; x < 2; x++)
  {
    for (int y = 0; y < 2; y++)
    {
      for (int z = 0; z < 2; z++)
      {
        Eigen::Vector3i current_idx = idx + Eigen::Vector3i(x, y, z);
        Eigen::Vector3d current_pos;
        indexToPos(current_idx, current_pos);
        pts[x][y][z] = current_pos;
      }
    }
  }
}

void GridMap::getSurroundDistance(Eigen::Vector3d pts[2][2][2],
                                  double dists[2][2][2])
{
  for (int x = 0; x < 2; x++)
  {
    for (int y = 0; y < 2; y++)
    {
      for (int z = 0; z < 2; z++)
      {
        dists[x][y][z] = getDistance(pts[x][y][z]);
      }
    }
  }
}

void GridMap::interpolateTrilinear(double values[2][2][2],
                                   const Eigen::Vector3d &diff,
                                   double &value)
{
  // trilinear interpolation
  double v00 = (1 - diff(0)) * values[0][0][0] + diff(0) * values[1][0][0];
  double v01 = (1 - diff(0)) * values[0][0][1] + diff(0) * values[1][0][1];
  double v10 = (1 - diff(0)) * values[0][1][0] + diff(0) * values[1][1][0];
  double v11 = (1 - diff(0)) * values[0][1][1] + diff(0) * values[1][1][1];
  double v0 = (1 - diff(1)) * v00 + diff(1) * v10;
  double v1 = (1 - diff(1)) * v01 + diff(1) * v11;

  value = (1 - diff(2)) * v0 + diff(2) * v1;
}

void GridMap::interpolateTrilinear(double values[2][2][2],
                                   const Eigen::Vector3d &diff,
                                   double &value,
                                   Eigen::Vector3d &grad)
{
  // trilinear interpolation
  double v00 = (1 - diff(0)) * values[0][0][0] + diff(0) * values[1][0][0];
  double v01 = (1 - diff(0)) * values[0][0][1] + diff(0) * values[1][0][1];
  double v10 = (1 - diff(0)) * values[0][1][0] + diff(0) * values[1][1][0];
  double v11 = (1 - diff(0)) * values[0][1][1] + diff(0) * values[1][1][1];
  double v0 = (1 - diff(1)) * v00 + diff(1) * v10;
  double v1 = (1 - diff(1)) * v01 + diff(1) * v11;

  value = (1 - diff(2)) * v0 + diff(2) * v1;

  // TODO 为什么梯度要乘resolution_inv_
  grad[2] = (v1 - v0) * gmp_.resolution_inv_;
  grad[1] = ((1 - diff[2]) * (v10 - v00) + diff[2] * (v11 - v01)) *
            gmp_.resolution_inv_;
  grad[0] = (1 - diff[2]) * (1 - diff[1]) * (values[1][0][0] - values[0][0][0]);
  grad[0] += (1 - diff[2]) * diff[1] * (values[1][1][0] - values[0][1][0]);
  grad[0] += diff[2] * (1 - diff[1]) * (values[1][0][1] - values[0][0][1]);
  grad[0] += diff[2] * diff[1] * (values[1][1][1] - values[0][1][1]);
  grad[0] *= gmp_.resolution_inv_;
}
